#include <string.h>
#include <ServoSoft.h>
#include <MsTimer2.h>
#include <stdlib.h>
#include <stdio.h>
#include <defArduinoParaArduino.h>

#define DEBUG 0
#define ELETROIMA 13
#define TOKE_EF 10
#define TOKE_BRACO 11

#define ZERO_ServoZ 94


/* Posicao inicial do scara (X=17,Y=0,Z=4) */
#define THETA1_INICIAL 119
#define THETA2_INICIAL 37
#define THETA_EF_INICIAL 47

int theta1_anterior = THETA1_INICIAL;
int theta2_anterior = THETA2_INICIAL;
int theta_ef_anterior = THETA_EF_INICIAL;

int v=0, i=0, fuck=1;
char ch;
int theta1 = THETA1_INICIAL, theta2 = THETA2_INICIAL, theta_ef = THETA_EF_INICIAL;
 
char msg[16];
char msgSize;
char response[16];

 Servo junta01;
 Servo junta02;
 Servo efetuador;
 Servo servoZ;
 Servo servoQqr;

int serialGetMsg()
{
  int i;  
  i=0;
  
  
  while (Serial.available())
    msg[i++] = Serial.read();
     
    
  if(i>0)
    msg[i]='\0';
  
  msgSize=i;
  
  return i;
}

void refreshServos()
{
  if( theta1 > theta1_anterior )
    junta01.write(++theta1_anterior);
  else if( theta1 < theta1_anterior )
          junta01.write(--theta1_anterior);
       
  if( theta2 > theta2_anterior )
    junta02.write(++theta2_anterior);
  else if( theta2 < theta2_anterior )
          junta02.write(--theta2_anterior);
       
/*       
  if( theta_ef > theta_ef_anterior )
    efetuador.write(++theta_ef_anterior);
  else if( theta_ef < theta_ef_anterior )
          efetuador.write(--theta_ef_anterior); */
  
  Servo::refresh();
}


void Z_cima( )
{
  int time = 0;
  
  time = (msg[1]-'0')*100;
  time = time + (msg[2]-'0')*10;
  time = time + (msg[3]-'0')*1;

  servoZ.write(ZERO_ServoZ-2);
  delay(time*100);
  servoZ.write(ZERO_ServoZ);
    
}

void Z_baixo( )
{
  int time = 0;
  
  time = (msg[1]-'0')*100;
  time = time + (msg[2]-'0')*10;
  time = time + (msg[3]-'0')*1;

  servoZ.write(ZERO_ServoZ+2);
  delay(time*100);
  servoZ.write(ZERO_ServoZ);
  
}


void deixaBloco( )
{
  digitalWrite(ELETROIMA,LOW);
  
  delay(300);
  
  servoZ.write(ZERO_ServoZ-2);
  
  while( digitalRead(TOKE_BRACO) != 1 )
   delay(3);
  
  servoZ.write(ZERO_ServoZ);
}
  

void pegaBloco( )
{
  servoZ.write(ZERO_ServoZ+2);
  
  while( digitalRead(TOKE_EF) != 1 )
    delay(3);
  
  servoZ.write(ZERO_ServoZ);
  
  digitalWrite(ELETROIMA,HIGH);
  
  delay(1500);
  
  servoZ.write(ZERO_ServoZ-2);
  
  while( digitalRead(TOKE_BRACO) != 1 )
    delay(3);
  
  servoZ.write(ZERO_ServoZ);
  
}


void leSensores()
{

char resposta[10];
char aux[5];
int ang = 1023;

resposta[0] = '<';
resposta[1] = '\0';

sprintf(aux,"%d",ang); // transforma o int em string
    
if ( strlen(aux) == 1 )
{
  strcpy(resposta,"<000");
  strcat(resposta,aux);
  
} else 
    if ( strlen(aux) == 2 )
    {
       strcpy(resposta,"<00");
       strcat(resposta,aux);
    }
    else 
      if( strlen(aux) == 3 )
      {
        strcpy(resposta,"<0");
        strcat(resposta,aux);
      }
      else
        strcat(resposta,aux);
        
 resposta[5] = '>';
 resposta[6] = '\0';

  Serial.println(resposta);
  
}



void setup() 
{ 
  
  MsTimer2::set(20,refreshServos);
  
  Serial.begin(115200);
  
  pinMode(TOKE_EF,INPUT);
  pinMode(ELETROIMA,OUTPUT);
  
  /* valores para o servo TowardPro MG946R */
  junta01.attach(2); 
  junta01.setMinimumPulse(900); 
  junta01.setMaximumPulse(2000);
  
  
  /* valores para o servo TowardPro MG946R */
  junta02.attach(3);
  junta02.setMinimumPulse(820); //original: 800
  junta02.setMaximumPulse(2200); //original 2200
  
  
  /* Valores ajustados para o servinho preto */
  efetuador.attach(4);
  efetuador.setMinimumPulse(650);
  efetuador.setMaximumPulse(2350);
  
  /* Esse servo eh CONTINUO */
  servoZ.attach(5);
  servoZ.setMinimumPulse(900);
  servoZ.setMaximumPulse(2000);
    
  MsTimer2::start();
} 
 

void mudaServos(  )
{
  int angulo = 0;
  int n;
  
  n = msg[1] - '0';
  angulo = 100*n;
  n = msg[2] - '0';
  angulo = angulo + 10*n;
  n = msg[3] - '0';
  angulo = angulo + 1*n;
  
  theta1 = angulo;
  angulo = 0;
  
  n = msg[4] - '0';
  angulo = 100*n;
  n = msg[5] - '0';
  angulo = angulo + 10*n;
  n = msg[6] - '0';
  angulo = angulo + 1*n;
  
  theta2 = angulo;
  angulo = 0;
  
  n = msg[7] - '0';
  angulo = 100*n;
  n = msg[8] - '0';
  angulo = angulo + 10*n;
  n = msg[9] - '0';
  angulo = angulo + 1*n;
  
  theta_ef = angulo;
    
}

void loop() 
{  
    
  if( fuck == 1) // soh entra uma vez nessa porra
  {
    junta01.write(THETA1_INICIAL);
    junta02.write(THETA2_INICIAL);
    //efetuador.write(THETA_EF_INICIAL);
     
    fuck=0;  
  }
    
  delay(5);// wait for serial input
    
  int n = serialGetMsg();
  
  if(n)
  {
    if( strlen(msg) > 10 )
    {
       if( (msg[0]=='<') && (msg[10]=='>') )
       {
         Serial.println("ACK");    
         mudaServos();
       }
       else
          Serial.println("NACK");
    }
    else 
      if( (msg[0]=='<') && (msg[5]=='>') )
         {
             Serial.println("ACK");    
           
              switch( msg[4] )
              {
                             
                  case SCARA_PEGA_BLOCO: pegaBloco( );
                                         break;  
                            
        
                  case SCARA_DEIXA_BLOCO: deixaBloco( );
                                          break;                      
        
                  case SCARA_DESCE_EF: Z_baixo();
                                       break; 
        
                  case SCARA_SOBE_EF: Z_cima();
                                       break; 
        
          
                  case SCARA_LE_SENSORES: leSensores();
                                          break;
                                          
                  case SCARA_DETACH_SERVOS: junta01.detach();
                                            junta02.detach();  
                                            efetuador.detach();
                                            servoZ.detach();
                                            break;
        
                  default : break;  
      
              } 
        
         }
         else
           Serial.println("NACK");
  }
  
      
  if( DEBUG )
  { 
    Serial.println("Angulos das juntas:");
    Serial.print("theta1: ");
    Serial.println(theta1);
    Serial.print("theta2: ");
    Serial.println(theta2);
    Serial.print("theta_ef: ");
    Serial.println(theta_ef);
    
    delay(1000);
   }
 
   delay(50);

} 
